.equ SRCPND,0x4a000000
.equ INTPND,0x4a000010

.equ rEINTPEND,0x560000a8
.equ INTOFFSET,0x4a000014

.equ USERMODE,0x10
.equ FIQMODE,0x11
.equ IRQMODE,0x12
.equ SVCMODE,0x13
.equ ABORTMODE,0x17
.equ UNDEFMODE,0x1b
.equ MODEMASK,0x1f
.equ NOINT,0xc0

    .text
.global CPU_IntDis
CPU_IntDis:
	MRS	R0,CPSR
	ORR	R0,R0,#0xC0
	MSR	CPSR_c,R0
	BX	LR

.global CPU_IntEn
CPU_IntEn:
	MRS	R0,CPSR
	AND	R1,R0,#0x3F
	MSR	CPSR_c,R1
	BX	LR


.global raw_start_first_task
raw_start_first_task:
		LDR R0, =raw_task_active
		LDR R0, [R0]         
		LDR SP, [R0]
		
		LDMFD 	SP!, {R0}  
		MSR 	SPSR_cxsf, R0
		LDMFD 	SP!, {R0-R12, LR, PC}^

.global port_task_switch
port_task_switch:
        STMFD SP!,{LR}              // push pc (lr should be pushed in place of PC)
        STMFD SP!,{R0-R12,LR}       // push lr & register file
        MRS   R0,CPSR
        STMFD SP!,{R0}              // push current cpsr
   
        // raw_task_active->task_stack = SP
		LDR	R0, =raw_task_active
		LDR	R0, [R0]
		STR	SP, [R0]

		PUSH    {R14}
		bl      raw_stack_check
		POP     {R14}

.global raw_int_switch
raw_int_switch:
	LDR		R0, =high_ready_obj
	LDR		R1, =raw_task_active
	LDR		R0, [R0]
	STR		R0, [R1]
		
	LDR		R0, =raw_task_active
	LDR		R0, [R0]
	LDR		SP, [R0]
	
	//----------------------------------------------------------------------------------	
	// Restore New Task context
	//----------------------------------------------------------------------------------	
	LDMFD 	SP!, {R0}              //POP CPSR
	MSR 	SPSR_cxsf, R0
	LDMFD 	SP!, {R0-R12, LR, PC}^	

.global OS_CPU_SR_Save
OS_CPU_SR_Save:
	MRS     R0, CPSR				// Set IRQ and FIQ bits in CPSR to disable all interrupts
	ORR     R1, R0, #0xC0
	MSR     CPSR_c, R1
	MRS     R1, CPSR				// Confirm that CPSR contains the proper interrupt disable flags
	AND     R1, R1, #0xC0
	CMP     R1, #0xC0
	BNE     OS_CPU_SR_Save				// Not properly disabled (try again)
	BX LR					// Disabled, return the original CPSR contents in R0

.global OS_CPU_SR_Restore
OS_CPU_SR_Restore:
	MSR     CPSR_c, R0
	BX 		LR

.global read_cpsr
read_cpsr:
	MRS R0, CPSR
	BX  LR

.global write_cpsr
write_cpsr:
	MSR   CPSR_cxsf,R0
	BX  LR

.global gorun
gorun:
    mov lr,r0
	bx lr


.global jump_to_dest
jump_to_dest:
	ldr   r0, =0x33400000;
	bx r0

.global raw_os_interrupt
raw_os_interrupt:
	STMFD   SP!, {R1-R3}			// We will use R1-R3 as temporary registers


	MOV     R1, SP
	ADD     SP, SP, #12             //Adjust IRQ stack pointer
	SUB     R2, LR, #4              //Adjust PC for return address to task

	MRS     R3, SPSR				// Copy SPSR (Task CPSR)
	
   

	MSR     CPSR_cxsf, #0xd3   //Change to SVC mode

									// SAVE TASK''S CONTEXT ONTO OLD TASK''S STACK
									
	STMFD   SP!, {R2}				// Push task''s PC 
	STMFD   SP!, {R4-R12, LR}		// Push task''s LR,R12-R4
	
	LDMFD   R1!, {R4-R6}			// Load Task''s R1-R3 from IRQ stack 
	STMFD   SP!, {R4-R6}			// Push Task''s R1-R3 to SVC stack
	STMFD   SP!, {R0}			    // Push Task''s R0 to SVC stack
	
	STMFD   SP!, {R3}				// Push task''s CPSR
	
	LDR     R4,=raw_task_active            
	LDR     R5,[R4]
	STR     SP,[R5] 

	PUSH    {R14}
	bl      raw_stack_check
	POP     {R14}
	
	BL raw_enter_interrupt	
	
	MSR    CPSR_c,#0xd2    //Change to IRQ mode to use IRQ stack to handle interrupt
		
	BL      irq_process
    MSR		CPSR_c,#0xd3   //Change to SVC mode
    BL raw_finish_int
    LDMFD   SP!,{R4}               //POP the task''s CPSR 
    MSR		SPSR_cxsf,R4
    LDMFD   SP!,{R0-R12,LR,PC}^	   //POP new Task''s context



